package jomodel;

import java.nio.FloatBuffer;

/**
 * A 3D matrix.  Includes utility functions to create common matrices and do matrix math.
 */ 
public class JOMatrix
{
	// 4x4 Matrix values
	public float m00=1, m01=0, m02=0, m03=0;
	public float m10=0, m11=1, m12=0, m13=0;
	public float m20=0, m21=0, m22=1, m23=0;
	public float m30=0, m31=0, m32=0, m33=1;


	public JOMatrix()
	{
	}

	public JOMatrix(FloatBuffer sourcematrix)
	{
		importFromBuffer(sourcematrix);
	}

	public JOMatrix(float[] sourcematrix)
	{
		importFromArray(sourcematrix);
	}

	public JOMatrix(JOVector right, JOVector up, JOVector forward)
	{
		// x axis
		m00 = right.x;
		m10 = right.y;
		m20 = right.z;
		// y axis
		m01 = up.x;
		m11 = up.y;
		m21 = up.z;
		// z axis
		m02 = forward.x;
		m12 = forward.y;
		m22 = forward.z;
	}

	public float[][] exportToArray()
	{
		float data[][]=new float[4][4];
		data[0][0]=m00;  data[0][1]=m01;  data[0][2]=m02;  data[0][3]=m03;
		data[1][0]=m10;  data[1][1]=m11;  data[1][2]=m12;  data[1][3]=m13;
		data[2][0]=m20;  data[2][1]=m21;  data[2][2]=m22;  data[2][3]=m23;
		data[3][0]=m30;  data[3][1]=m31;  data[3][2]=m32;  data[3][3]=m33;
		return data;
	}

	public JOMatrix importFromArray(float[][] data)
	{
		if (data.length<4) {
			return this;
		}
		for (int i=0;i<4;i++) {
			if (data[i].length<4) return this;
		}
		m00=data[0][0];  m01=data[0][1];  m02=data[0][2];  m03=data[0][3];
		m10=data[1][0];  m11=data[1][1];  m12=data[1][2];  m13=data[1][3];
		m20=data[2][0];  m21=data[2][1];  m22=data[2][2];  m23=data[2][3];
		m30=data[3][0];  m31=data[3][1];  m32=data[3][2];  m33=data[3][3];
		return this;
	}

	public JOMatrix importFromArray(float[] data)
	{
		if (data.length < 16) {
			return this;
		}
		m00=data[0];  m01=data[1];  m02=data[2];  m03=data[3];
		m10=data[4];  m11=data[5];  m12=data[6];  m13=data[7];
		m20=data[8];  m21=data[9];  m22=data[10];  m23=data[11];
		m30=data[12];  m31=data[13];  m32=data[14];  m33=data[15];
		return this;
	}

    /**
     * Convert a FloatBuffer matrix to a 4x4 float array.
     * @param fb   FloatBuffer containing 16 values of 4x4 matrix
     * @return     2 dimensional float array
     */
    public JOMatrix importFromBuffer(FloatBuffer fb) {
    	//fb.rewind();
        m00 = fb.get(0);
        m01 = fb.get(1);
        m02 = fb.get(2);
        m03 = fb.get(3);
        m10 = fb.get(4);
        m11 = fb.get(5);
        m12 = fb.get(6);
        m13 = fb.get(7);
        m20 = fb.get(8);
        m21 = fb.get(9);
        m22 = fb.get(10);
        m23 = fb.get(11);
        m30 = fb.get(12);
        m31 = fb.get(13);
        m32 = fb.get(14);
        m33 = fb.get(15);
        return this;
    }

    /**
     * fill the given float array with the matrix values.
     * @param fa   float[16]
     */
    public void exportToArray(float[] fa) {
        if (fa.length < 16) {
        	return;
        }
        fa[0] = m00;
        fa[1] = m01;
        fa[2] = m02;
        fa[3] = m03;
        
        fa[4] = m10;
        fa[5] = m11;
        fa[6] = m12;
        fa[7] = m13;
        
        fa[8] = m20;
        fa[9] = m21;
        fa[10] = m22;
        fa[11] = m23;
        
        fa[12] = m30;
        fa[13] = m31;
        fa[14] = m32;
        fa[15] = m33;
    }

    public void exportToBuffer(FloatBuffer fb) {
        if (fb.capacity() < 16) {
        	return;
        }
        fb.put(0,m00);
        fb.put(1,m01);
        fb.put(2,m02);
        fb.put(3,m03);
        
        fb.put(4,m10);
        fb.put(5,m11);
        fb.put(6,m12);
        fb.put(7,m13);
        
        fb.put(8,m20);
        fb.put(9,m21);
        fb.put(10,m22);
        fb.put(11,m23);
        
        fb.put(12,m30);
        fb.put(13,m31);
        fb.put(14,m32);
        fb.put(15,m33);
    }

	//--------------------------------------------------------------------
	// Methods to create typical matrices
	//--------------------------------------------------------------------

	// matrix for shifting
	public static JOMatrix translateMatrix(float dx, float dy, float dz)
	{
		JOMatrix m=new JOMatrix();
		m.m03=dx;
		m.m13=dy;
		m.m23=dz;
		return m;
	}

	// matrix for scaling
	public static JOMatrix scaleMatrix(float dx, float dy, float dz)
	{
		JOMatrix m=new JOMatrix();
		m.m00=dx;
		m.m11=dy;
		m.m22=dz;
		return m;
	}

	// matrix for scaling
	public static JOMatrix scaleMatrix(float d)
	{
		return JOMatrix.scaleMatrix(d,d,d);
	}

	// matrix for rotation
	public static JOMatrix rotateMatrix(float dx, float dy, float dz)
	{
		JOMatrix out=new JOMatrix();
		float SIN;
		float COS;

		if (dx!=0)
		{
			JOMatrix m =new JOMatrix();
			SIN = (float)Math.sin(dx);
			COS = (float)Math.cos(dx);
			m.m11=COS;
			m.m12=SIN;
			m.m21=-SIN;
			m.m22=COS;
			out.transform(m);
		}
		if (dy!=0)
		{
			JOMatrix m =new JOMatrix();
			SIN = (float)Math.sin(dy);
			COS = (float)Math.cos(dy);
			m.m00=COS;
			m.m02=SIN;
			m.m20=-SIN;
			m.m22=COS;
			out.transform(m);
		}
		if (dz!=0)
		{
			JOMatrix m =new JOMatrix();
			SIN = (float)Math.sin(dz);
			COS = (float)Math.cos(dz);
			m.m00=COS;
			m.m01=SIN;
			m.m10=-SIN;
			m.m11=COS;
			out.transform(m);
		}
		return out;
	}

    /**
     * Create a matrix from an arbitrary set of axes and return it in the given
     * floatbuffer. The three axis vectors are the right (X), up (Y), and look (Z). 
     *
     * The axis values are stored in the first 3 columns of the matrix.
     * The translate position is in the last column, so the matrix will position
     * the coordinate system as well as orient it.
     *
     * @param matrix  FloatBuffer where the matrix will be returned
     * @param right   which way is right
     * @param up      which way is up
     * @param look    which way is forward
     * @param pos     position where you want geometry to be placed
     */
    public static void createMatrix(FloatBuffer matrix, JOVector right, JOVector up, JOVector look, JOVector pos)
    {
    	// x axis
    	matrix.put(0,  right.x);
    	matrix.put(1,  right.y);
    	matrix.put(2,  right.z);
    	matrix.put(3,  0);

    	// y axis
    	matrix.put(4,  up.x);
    	matrix.put(5,  up.y);
    	matrix.put(6,  up.z);
    	matrix.put(7,  0);

    	// z axis
    	matrix.put(8,  look.x);
    	matrix.put(9,  look.y);
    	matrix.put(10, look.z);
    	matrix.put(11, 0);

    	// Add the translation
    	matrix.put(12, pos.x);
    	matrix.put(13, pos.y);
    	matrix.put(14, pos.z);
    	matrix.put(15, 1);
    }

    //--------------------------------------------------------------------
	// Public methods
	//--------------------------------------------------------------------

	public void translate(float dx, float dy, float dz)
	{
		transform(translateMatrix(dx,dy,dz));
	}

	public void scale(float dx, float dy, float dz)
	{
		transform(scaleMatrix(dx,dy,dz));
	}

	public void scale(float d)
	{
		transform(scaleMatrix(d));
	}

	public void rotate(float dx, float dy, float dz)
	{
		transform(rotateMatrix(dx,dy,dz));
	}

	public void scaleSelf(float dx, float dy, float dz)
	{
		preTransform(scaleMatrix(dx,dy,dz));
	}

	public void scaleSelf(float d)
	{
		preTransform(scaleMatrix(d));
	}

	public void rotateSelf(float dx, float dy, float dz)
	{
		preTransform(rotateMatrix(dx,dy,dz));
	}

	/**
	 *  Transform the given vector using this matrix. Return the
	 *  transformed vector (the original vector is not modified).
	 *
	 *  @param v   JOVector to be transformed
	 *  @return    the transformed JOVector
	 */
	public JOVector transform(JOVector v)
	{
		if (v != null) {
			float newx, newy, newz;
			newx = v.x*m00 + v.y*m01 + v.z*m02+ m03;
			newy = v.x*m10 + v.y*m11 + v.z*m12+ m13;
			newz = v.x*m20 + v.y*m21 + v.z*m22+ m23;
			return new JOVector(newx,newy,newz);
		}
		return null;
	}
	
	// transforms this matrix by matrix n from left (this=n x this)
	public void transform(JOMatrix n)
	{
		JOMatrix m=this.getClone();

		m00 = n.m00*m.m00 + n.m01*m.m10 + n.m02*m.m20;
		m01 = n.m00*m.m01 + n.m01*m.m11 + n.m02*m.m21;
		m02 = n.m00*m.m02 + n.m01*m.m12 + n.m02*m.m22;
		m03 = n.m00*m.m03 + n.m01*m.m13 + n.m02*m.m23 + n.m03;
		m10 = n.m10*m.m00 + n.m11*m.m10 + n.m12*m.m20;
		m11 = n.m10*m.m01 + n.m11*m.m11 + n.m12*m.m21;
		m12 = n.m10*m.m02 + n.m11*m.m12 + n.m12*m.m22;
		m13 = n.m10*m.m03 + n.m11*m.m13 + n.m12*m.m23 + n.m13;
		m20 = n.m20*m.m00 + n.m21*m.m10 + n.m22*m.m20;
		m21 = n.m20*m.m01 + n.m21*m.m11 + n.m22*m.m21;
		m22 = n.m20*m.m02 + n.m21*m.m12 + n.m22*m.m22;
		m23 = n.m20*m.m03 + n.m21*m.m13 + n.m22*m.m23 + n.m23;
	}

	// transforms this matrix by matrix n from right (this=this x n)
	public void preTransform(JOMatrix n)
	{
		JOMatrix m=this.getClone();

		m00 = m.m00*n.m00 + m.m01*n.m10 + m.m02*n.m20;
		m01 = m.m00*n.m01 + m.m01*n.m11 + m.m02*n.m21;
		m02 = m.m00*n.m02 + m.m01*n.m12 + m.m02*n.m22;
		m03 = m.m00*n.m03 + m.m01*n.m13 + m.m02*n.m23 + m.m03;
		m10 = m.m10*n.m00 + m.m11*n.m10 + m.m12*n.m20;
		m11 = m.m10*n.m01 + m.m11*n.m11 + m.m12*n.m21;
		m12 = m.m10*n.m02 + m.m11*n.m12 + m.m12*n.m22;
		m13 = m.m10*n.m03 + m.m11*n.m13 + m.m12*n.m23 + m.m13;
		m20 = m.m20*n.m00 + m.m21*n.m10 + m.m22*n.m20;
		m21 = m.m20*n.m01 + m.m21*n.m11 + m.m22*n.m21;
		m22 = m.m20*n.m02 + m.m21*n.m12 + m.m22*n.m22;
		m23 = m.m20*n.m03 + m.m21*n.m13 + m.m22*n.m23 + m.m23;
	}

	// returns m1 x m2
	public static JOMatrix multiply(JOMatrix m1, JOMatrix m2)
	{
		JOMatrix m=new JOMatrix();

		m.m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20;
		m.m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21;
		m.m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22;
		m.m03 = m1.m00*m2.m03 + m1.m01*m2.m13 + m1.m02*m2.m23 + m1.m03;
		m.m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20;
		m.m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21;
		m.m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22;
		m.m13 = m1.m10*m2.m03 + m1.m11*m2.m13 + m1.m12*m2.m23 + m1.m13;
		m.m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20;
		m.m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21;
		m.m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22;
		m.m23 = m1.m20*m2.m03 + m1.m21*m2.m13 + m1.m22*m2.m23 + m1.m23;
		return m;
	}

	public String toString()
	{
		StringBuffer out=new StringBuffer("<Matrix: \r\n");
		out.append(m00+","+m01+","+m02+","+m03+",\r\n");
		out.append(m10+","+m11+","+m12+","+m13+",\r\n");
		out.append(m20+","+m21+","+m22+","+m23+",\r\n");
		out.append(m30+","+m31+","+m32+","+m33+">\r\n");
		return out.toString();
	}

	public JOMatrix getClone()
	{
		JOMatrix m=new JOMatrix();
		m.m00=m00;  m.m01=m01;  m.m02=m02;  m.m03=m03;
		m.m10=m10;  m.m11=m11;  m.m12=m12;  m.m13=m13;
		m.m20=m20;  m.m21=m21;  m.m22=m22;  m.m23=m23;
		m.m30=m30;  m.m31=m31;  m.m32=m32;  m.m33=m33;
		return m;
	}

	// Returns the inverse of this matrix
	// Code generated with MapleV and handoptimized
	public JOMatrix inverse()
	{
		JOMatrix m = new JOMatrix();

		float q1 = m12;  float q6 = m10*m01;  float q7 = m10*m21;  float q8 = m02;
		float q13 = m20*m01;  float q14 = m20*m11;  float q21 = m02*m21;  float q22 = m03*m21;
		float q25 = m01*m12;  float q26 = m01*m13;  float q27 = m02*m11;  float q28 = m03*m11;
		float q29 = m10*m22;  float q30 = m10*m23;  float q31 = m20*m12;  float q32 = m20*m13;
		float q35 = m00*m22;  float q36 = m00*m23;  float q37 = m20*m02;  float q38 = m20*m03;
		float q41 = m00*m12;  float q42 = m00*m13;  float q43 = m10*m02;  float q44 = m10*m03;
		float q45 = m00*m11;  float q48 = m00*m21;
		float q49 = q45*m22-q48*q1-q6*m22+q7*q8;
		float q50 = q13*q1-q14*q8;
		float q51 = 1/(q49+q50);

		m.m00 = (m11*m22*m33-m11*m23*m32-m21*m12*m33+m21*m13*m32+m31*m12*m23-m31*m13*m22)*q51;
		m.m01 = -(m01*m22*m33-m01*m23*m32-q21*m33+q22*m32)*q51;
		m.m02 = (q25*m33-q26*m32-q27*m33+q28*m32)*q51;
		m.m03 = -(q25*m23-q26*m22-q27*m23+q28*m22+q21*m13-q22*m12)*q51;
		m.m10 = -(q29*m33-q30*m32-q31*m33+q32*m32)*q51;
		m.m11 = (q35*m33-q36*m32-q37*m33+q38*m32)*q51;
		m.m12 = -(q41*m33-q42*m32-q43*m33+q44*m32)*q51;
		m.m13 = (q41*m23-q42*m22-q43*m23+q44*m22+q37*m13-q38*m12)*q51;
		m.m20 = (q7*m33-q30*m31-q14*m33+q32*m31)*q51;
		m.m21 = -(q48*m33-q36*m31-q13*m33+q38*m31)*q51;
		m.m22 = (q45*m33-q42*m31-q6*m33+q44*m31)*q51;
		m.m23 = -(q45*m23-q42*m21-q6*m23+q44*m21+q13*m13-q38*m11)*q51;

		return m;
	}

	// Resets the matrix
	public void reset()
	{
		m00=1; m01=0; m02=0; m03=0;
		m10=0; m11=1; m12=0; m13=0;
		m20=0; m21=0; m22=1; m23=0;
		m30=0; m31=0; m32=0; m33=1;
	}
	
 	public JOMatrix invert()
 	{
 		JOMatrix.invert(this);
 		return this;
 	}

 	public static void invert(JOMatrix src)
 	{
 		float[] srcA = new float[16];
 		float[] inverseA = new float[16];
 		src.exportToArray(srcA);
 		invert(srcA,inverseA);
 		src.importFromArray(inverseA);
 	}

 	// TEST diff't invert function
 	public static void invert(float src[], float inverse[])
 	{
 	    float t;
 	    int i, j, k, swap;
 	    float tmp[][] = new float[4][4];
 	    
		inverse[0]=1; inverse[1]=0; inverse[2]=0; inverse[3]=0; 
		inverse[4]=0; inverse[5]=1; inverse[6]=0; inverse[7]=0;
		inverse[8]=0; inverse[9]=0; inverse[10]=1; inverse[11]=0;
		inverse[12]=0; inverse[13]=0; inverse[14]=0; inverse[15]=1;

 	    for (i = 0; i < 4; i++) {
 	        for (j = 0; j < 4; j++) {
 	            tmp[i][j] = src[i*4+j];
 	        }
 	    }
 	    
 	    for (i = 0; i < 4; i++) {
 	        /* look for largest element in column. */
 	        swap = i;
 	        for (j = i + 1; j < 4; j++) {
 	            if (Math.abs(tmp[j][i]) > Math.abs(tmp[i][i])) {
 	                swap = j;
 	            }
 	        }
 	        
 	        if (swap != i) {
 	            /* swap rows. */
 	            for (k = 0; k < 4; k++) {
 	                t = tmp[i][k];
 	                tmp[i][k] = tmp[swap][k];
 	                tmp[swap][k] = t;
 	                
 	                t = inverse[i*4+k];
 	                inverse[i*4+k] = inverse[swap*4+k];
 	                inverse[swap*4+k] = t;
 	            }
 	        }
 	        
 	        if (tmp[i][i] == 0) {
 	        /* no non-zero pivot.  the matrix is singular, which
 	           shouldn't happen.  This means the user gave us a bad
 	            matrix. */
 	        	System.out.println("ACK");
 	            return;
 	        }
 	        
 	        t = tmp[i][i];
 	        for (k = 0; k < 4; k++) {
 	            tmp[i][k] /= t;
 	            inverse[i*4+k] /= t;
 	        }
 	        for (j = 0; j < 4; j++) {
 	            if (j != i) {
 	                t = tmp[j][i];
 	                for (k = 0; k < 4; k++) {
 	                    tmp[j][k] -= tmp[i][k]*t;
 	                    inverse[j*4+k] -= inverse[i*4+k]*t;
 	                }
 	            }
 	        }
 	    }
 	}

 	public JOMatrix transpose(){
 		JOMatrix t = new JOMatrix();
 		JOMatrix m = this;
 		t.m00=m.m00;  t.m01=m.m10;  t.m02=m.m20;  t.m03=m.m30;
 		t.m10=m.m01;  t.m11=m.m11;  t.m12=m.m21;  t.m13=m.m31;
 		t.m20=m.m02;  t.m21=m.m12;  t.m22=m.m22;  t.m23=m.m32;
 		t.m30=m.m03;  t.m31=m.m13;  t.m32=m.m23;  t.m33=m.m33;
 		return t;
 	}

}